#include "Gyroscope.h"
#include "math.h"
#include "CarConfig.h"



GpyoInfo Gpyoscopedata = { 0 };

float GpyoZlast = 0.0f;
float GpyoZMid = 0.0f;


float GpyoZoffest = 0.0f;

void GyroscopeInit()
{
    imu963ra_init();
    for (int i = 0; i < 1000; ++i) {
        imu963ra_get_gyro();
        GpyoZoffest += imu963ra_gyro_transition(imu963ra_gyro_z);
        system_delay_ms(1);
    }
    GpyoZoffest /= 1000.f;
}


double LowPassFilter_Silding(double dataNewest, double dataMiddle, double dataLast)
{
    double result;
    result = 0.5f * dataNewest + 0.3f * dataMiddle + 0.2f * dataLast;
    return result;
}

void GetGyroscopedata()
{
    imu963ra_get_gyro();
    GpyoZlast = GpyoZMid;
    GpyoZMid = Gpyoscopedata.GpyoZ;
    Gpyoscopedata.GpyoZ = imu963ra_gyro_transition(imu963ra_gyro_z) - GpyoZoffest;
    float flinal = LowPassFilter_Silding(Gpyoscopedata.GpyoZ, GpyoZMid, GpyoZlast);
    Gpyoscopedata.AngZ += flinal * (float) PIT_TIME / 1000.0f;
}

void SendGyroscopeData(size_t(*fun)(const char*, ...))
{
    fun("%f,%f,%f,", Gpyoscopedata.AccX, Gpyoscopedata.AccY, Gpyoscopedata.AccZ);
    fun("%f,%f,%f,", Gpyoscopedata.GpyoX, Gpyoscopedata.GpyoY, Gpyoscopedata.GpyoZ);
    fun("%f,%f,%f\n", Gpyoscopedata.AngX, Gpyoscopedata.AngY, Gpyoscopedata.AngZ);
}

